#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "pid.h"
#include "main.h"
#include "control.h"
#include "ModbusRegister.h"


void PID_Init(PID_SET *pid, float kp, float ki, float kd, uint8_t direct, float base)
{
  pid->Kp= kp;
  pid->Ki= ki;
  pid->Kd= kd;
  
  pid->err_0= 0.0f;
  pid->err_1= 0.0f;
  pid->err_2= 0.0f;
  
  pid->output= 0.0f;
  pid->direct= direct;
}

void PID_SetCoeff(PID_SET *pid, float kp, float ki, float kd)
{
  pid->Kp= kp;
  pid->Ki= ki;
  pid->Kd= kd;
}

float PID_Control(PID_SET *pid, float pv, float setpt, uint8_t tick, uint8_t defaultDelay, uint8_t freq_hold)
{
  float out_plus;

  if (pid->delay > 0) 
	{
    pid->delay -= tick;
  }
  InputReg[REG_PID_TICK] = pid->delay;
  if (pid->delay > 0)
  {
    return pid->output;
  }
  pid->delay = defaultDelay;
  
  if(pid->direct)
  {
    pid->err_0= pv - setpt;
  }
  else
  {
    pid->err_0= setpt - pv;
  }
  
  out_plus= pid->Kp*(pid->err_0 - pid->err_1) + pid->Kd*(pid->err_0 - 2.0f*pid->err_1 + pid->err_2);
    
  if(!((pid->output >= 1.0f && pid->err_0 > 0.0f) || (pid->output <= 0.0f && pid->err_0 < 00.0f)))
  {
    out_plus+= pid->Ki*pid->err_0;
  }
  
	if(out_plus < 0)
	{
		freq_hold = 0;
	}
	
	if(freq_hold == 0)
	{
		pid->output+= out_plus;
	}
  
  pid->err_2= pid->err_1;
  pid->err_1= pid->err_0;
  if(pid->output > 1.0f)
	{
			pid->output = 1.0f;
	}
	else if (pid->output < 0.0f)
	{
		pid->output = 0.0f;
	
	}
 // pid->output= _hi_lo_limit(pid->output, 1.0f, 0.0f);
  
  return pid->output;
}
